function [P, K, lambda, mu] = NMPC_get_RIS (A, B, Bw)
%% Get the RIS 
% By Zehua Jia, jiazehua@sjtu.edu.cn

%% System description
% x' = A * x + g(x) + B * u + Bw * w
% g(x) = [0; -0.25 * x2^3];
% The designed local state-feeback control law is u = K * x.

%% Some Tips
% A. The constraints should be Linear, which means nonlinear terms are not
% permitted (such as x*y and x^2, where x,y are both sdpvars). 
% 在使用 YALMIP 工具箱求解优化问题时，所需的约束必须是线性的，不允许出现非线性项。

%% Solve the optimization problem using YALMIP
% YALMIP提供了一个高层次的MATLAB接口，使用户能够轻松地定义和解决各种凸优化问题，包括线性规划、二次规划、半定规划等。
% 使用 YALMIP 工具箱定义了优化问题的决策变量和约束。
% 具体来说，使用了矩阵变量X和Y，以及标量变量μ来构建线性矩阵不等式（LMI）。

% Define decision matrix variables
[n,~] = size(A);
[~,m] = size(B);
mu = sdpvar(1); % 标量，通过sdpvar定义，用于描述鲁棒性的界限
X = diag(sdpvar(n,1)); % 对角矩阵，通过sdpvar定义，表示鲁棒不变集的形状
Y = sdpvar(m,n,'full'); % 矩阵，通过sdpvar定义，用于计算控制器参数
% lambda0 = sdpvar(1); 
lambda0 = 1.8; % 给定的常数

% Define LMI constraints(论文Lemma 2.)
% 构建线性矩阵不等式 LMI，包括状态方程和鲁棒性项，以及添加相关约束，构建了一个优化问题。
LMI = [(A * X + B * Y)' + A * X + B * Y + lambda0 * X, Bw; Bw', -mu];
% const = [LMI <= 0, X >= 0, mu >=0, lambda0 >= 1.8];
const = [LMI <= 0, X >= 0, mu >=0];

%% Solving
% 只需要找到满足一组约束的可行解而不是优化某个目标函数，可以使用solvesdp(const)这种形式。
% 而如果需要最小化或最大化某个目标函数，则使用optimize(const, obj, sdpsettings('solver','Mosek'))来明确指定优化目标和求解器
obj = 0; % new -X or trace(-X)
ops = sdpsettings('solver','Mosek', 'verbose', 0);
optimize(const,obj,ops);% 最小化obj

% solvesdp(const) % 通过solvesdp函数求解优化问题

%% Get results
X1 = value(X); % 通过value函数获取最优解，分别代表鲁棒不变集的形状和控制器参数
Y1 = value(Y);
P = X1^(-1); % X1的逆矩阵，用于计算鲁棒不变集
K = Y1 * P; % 控制器参数
% lambda = value(lambda0);
lambda = lambda0; % 给定的常数
mu = value(mu); % 代表了鲁棒性的界限
end
